﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using TomanuExtensions;
using System.Diagnostics;
using System.Collections;
using System.Globalization;

namespace RaytracerLib.MathLib.Polynomials
{
    [DebuggerStepThrough]
    public struct Polynomial
    {
        public static readonly Polynomial ZERO = new Polynomial(0.0);
        public static readonly Polynomial ONE = new Polynomial(1.0);

        private int m_order;
        private readonly double[] m_coeffs;

        public Polynomial(Polynomial a_poly)
        {
            m_coeffs = new double[a_poly.m_order + 1];
            m_order = a_poly.m_order;

            Array.Copy(a_poly.m_coeffs, m_coeffs, m_coeffs.Length);
        }

        private Polynomial(int a_order)
        {
            Debug.Assert(a_order >= 0);

            m_order = a_order;
            m_coeffs = new double[m_order + 1];
                
            Check();
        }

        public Polynomial(double a_c0)
        {
            m_coeffs = new double[1];
            m_order = 0;
            m_coeffs[0] = a_c0;

            Check();
        }

        public Polynomial(double a_c0, double a_c1)
        {
            m_coeffs = new double[2];
            m_order = 1;
            m_coeffs[0] = a_c0;
            m_coeffs[1] = a_c1;

            OptimizeOrder();

            Check();
        }

        public Polynomial(double a_c0, double a_c1, double a_c2)
        {
            m_coeffs = new double[3];
            m_order = 2;
            m_coeffs[0] = a_c0;
            m_coeffs[1] = a_c1;
            m_coeffs[2] = a_c2;

            OptimizeOrder();

            Check();
        }

        public Polynomial(double a_c0, double a_c1, double a_c2, double a_c3)
        {
            m_coeffs = new double[4];
            m_order = 3;
            m_coeffs[0] = a_c0;
            m_coeffs[1] = a_c1;
            m_coeffs[2] = a_c2;
            m_coeffs[3] = a_c3;

            OptimizeOrder();

            Check();
        }

        public Polynomial(double a_c0, double a_c1, double a_c2, double a_c3, 
            double a_c4)
        {
            m_coeffs = new double[5];
            m_order = 4;
            m_coeffs[0] = a_c0;
            m_coeffs[1] = a_c1;
            m_coeffs[2] = a_c2;
            m_coeffs[3] = a_c3;
            m_coeffs[4] = a_c4;

            OptimizeOrder();

            Check();
        }

        public Polynomial(double a_c0, double a_c1, double a_c2, double a_c3, 
            double a_c4, double a_c5)
        {
            m_coeffs = new double[6];
            m_order = 5;
            m_coeffs[0] = a_c0;
            m_coeffs[1] = a_c1;
            m_coeffs[2] = a_c2;
            m_coeffs[3] = a_c3;
            m_coeffs[4] = a_c4;
            m_coeffs[5] = a_c5;

            OptimizeOrder();

            Check();
        }

        public Polynomial(double a_c0, double a_c1, double a_c2, double a_c3, 
            double a_c4, double a_c5, double a_c6)
        {
            m_coeffs = new double[7];
            m_order = 6;
            m_coeffs[0] = a_c0;
            m_coeffs[1] = a_c1;
            m_coeffs[2] = a_c2;
            m_coeffs[3] = a_c3;
            m_coeffs[4] = a_c4;
            m_coeffs[5] = a_c5;
            m_coeffs[6] = a_c6;

            OptimizeOrder();

            Check();
        }

        public Polynomial(params double[] a_coeffs)
        {
            Debug.Assert(a_coeffs.Length > 0);

            m_coeffs = new double[a_coeffs.Length];
            m_order = m_coeffs.Length - 1;
            Array.Copy(a_coeffs, m_coeffs, m_coeffs.Length);

            OptimizeOrder();

            Check();
        }

        public override bool Equals(object a_obj)
        {
            if (a_obj == null)
                return false;

            if (!typeof(Polynomial).Equals(a_obj.GetType()))
                return false;
            Polynomial poly = (Polynomial)a_obj;

            if (m_order != poly.m_order)
                return false;

            for (int i = 0; i <= m_order; i++)
            {
                if (m_coeffs[i] != poly.m_coeffs[i])
                    return false;
            }

            return true;
        }

        public bool Equals(Polynomial a_poly)
        {
            if (m_order != a_poly.m_order)
                return false;

            for (int i = 0; i <= m_order; i++)
            {
                if (m_coeffs[i] != a_poly.m_coeffs[i])
                    return false;
            }

            return true;
        }

        public bool IsAlmostRelativeEquals(Polynomial a_poly, 
            double a_precision = Constants.DOUBLE_PRECISION)
        {
            if (a_poly.m_order > m_order)
            {
                for (int i = m_order + 1; i <= a_poly.m_order; i++)
                {
                    if (!a_poly.m_coeffs[i].IsAlmostRelativeEquals(0, a_precision))
                        return false;
                }
            }

            if (m_order > a_poly.m_order)
            {
                for (int i = a_poly.m_order + 1; i <= m_order; i++)
                {
                    if (!m_coeffs[i].IsAlmostRelativeEquals(0, a_precision))
                        return false;
                }
            }

            for (int i = 0; i <= Math.Min(m_order, a_poly.m_order); i++)
            {
                if (!m_coeffs[i].IsAlmostRelativeEquals(a_poly.m_coeffs[i],
                    a_precision))
                {
                    return false;
                }
            }

            return true;
        }

        public bool IsAlmostEquals(Polynomial a_poly, 
            double a_precision = Constants.DOUBLE_PRECISION)
        {
            if (a_poly.m_order > m_order)
            {
                for (int i = m_order + 1; i <= a_poly.m_order; i++)
                {
                    if (!a_poly.m_coeffs[i].IsAlmostEquals(0, a_precision))
                        return false;
                }
            }

            if (m_order > a_poly.m_order)
            {
                for (int i = a_poly.m_order + 1; i <= m_order; i++)
                {
                    if (!m_coeffs[i].IsAlmostEquals(0, a_precision))
                        return false;
                }
            }

            for (int i = 0; i <= Math.Min(m_order, a_poly.m_order); i++)
            {
                if (!m_coeffs[i].IsAlmostEquals(a_poly.m_coeffs[i], a_precision))
                    return false;
            }

            return true;
        }

        //

        public override int GetHashCode()
        {
            return ArrayExtensions.GetHashCode(m_coeffs);
        }

        public static bool operator !=(Polynomial a_a, Polynomial a_b)
        {
            if (a_a.m_order != a_b.m_order)
                return true;

            for (int i = 0; i <= a_a.m_order; i++)
            {
                if (a_a.m_coeffs[i] != a_b.m_coeffs[i])
                    return true;
            }

            return false;
        }

        public static bool operator ==(Polynomial a_a, Polynomial a_b)
        {
            if (a_a.m_order != a_b.m_order)
                return false;

            for (int i = 0; i <= a_a.m_order; i++)
            {
                if (a_a.m_coeffs[i] != a_b.m_coeffs[i])
                    return false;
            }

            return true;
        }

        [Conditional("DEBUG")]
        private void Check()
        {
            for (int i = 0; i <= m_order; i++)
                Debug.Assert(m_coeffs[i].IsNumber());

            Debug.Assert(m_order >= 0);
            Debug.Assert(m_coeffs.Length >= 1);
        }

        public int Order
        {
            get
            {
                return m_order;
            }
        }

        public double this[int a_index]
        {
            get
            {
                if (a_index > m_order)
                    throw new IndexOutOfRangeException();

                return m_coeffs[a_index];
            }
            private set
            {
                if (a_index > m_order)
                    throw new IndexOutOfRangeException();

                m_coeffs[a_index] = value;

                OptimizeOrder();

                Check();
            }
        }

        public double LeadCoefficient
        {
            get
            {
                return m_coeffs[m_order];
            }
        }

        public double LastCoefficient
        {
            get
            {
                return m_coeffs[0];
            }
        }

        public Polynomial Normalize()
        {
            if (m_order == 0)
                return Polynomial.ONE;

            double div = m_coeffs[m_order];

            if (div == 1)
                return this;

            Polynomial result = new Polynomial(this);

            for (int i = 0; i < result.m_order; i++)
                result.m_coeffs[i] /= div;

            result.m_coeffs[m_order] = 1;

            return result;  
        }

        public double Evaluate(double a_x)
        {
            double r = m_coeffs[m_order];

            for (int i = m_order - 1; i >= 0; i--)
                r = m_coeffs[i] + r * a_x;

            return r;
        }

        private void OptimizeOrder()
        {
            while (m_order > 0 && (m_coeffs[m_order] == 0.0))
                m_order--;
        }

        public static Polynomial operator * (Polynomial a_left, double a_right)
        {
            Polynomial result = new Polynomial(a_left);

            for (int i = 0; i <= result.m_order; i++)
                result.m_coeffs[i] *= a_right;

            result.OptimizeOrder();

            return result;
        }

        public static Polynomial operator * (double a_left, Polynomial a_right)
        {
            Polynomial result = new Polynomial(a_right);

            for (int i = 0; i <= result.m_order; i++)
                result.m_coeffs[i] *= a_left;

            result.OptimizeOrder();

            return result;
        }

        public static Polynomial operator /(Polynomial a_left, double a_right)
        {
            Polynomial result = new Polynomial(a_left);

            for (int i = 0; i <= result.m_order; i++)
                result.m_coeffs[i] /= a_right;

            result.OptimizeOrder();

            return result;
        }

        public Polynomial Differentiate()
        {
            if (m_order == 0)
                return Polynomial.ZERO;

            Polynomial result = new Polynomial(m_order - 1);

            for (int i = 1; i <= m_order; i++)
                result.m_coeffs[i - 1] += i * m_coeffs[i];

            return result;
        }

        public static Polynomial operator *(Polynomial a_u, Polynomial a_v)
        {
            Polynomial result = new Polynomial(a_u.m_order + a_v.m_order);

            for (int i = 0; i <= a_u.m_order; i++)
                for (int j = 0; j <= a_v.m_order; j++)
                    result.m_coeffs[i + j] += a_u.m_coeffs[i] * a_v.m_coeffs[j];

            result.OptimizeOrder();

            return result;
        }

        public static Polynomial operator +(Polynomial a_u, Polynomial a_v)
        {
            if (a_u.m_order == a_v.m_order)
            {
                Polynomial result = new Polynomial(a_u.m_order);

                for (int i = 0; i <= a_u.m_order; i++)
                    result.m_coeffs[i] = a_u.m_coeffs[i] + a_v.m_coeffs[i];

                result.OptimizeOrder();

                return result;
            }
            else if (a_u.m_order < a_v.m_order)
            {
                Polynomial result = new Polynomial(a_v.m_order);

                for (int i = a_u.m_order + 1; i <= a_v.m_order; i++)
                    result.m_coeffs[i] = a_v.m_coeffs[i];

                for (int i = 0; i <= a_u.m_order; i++)
                    result.m_coeffs[i] = a_u.m_coeffs[i] + a_v.m_coeffs[i];

                result.OptimizeOrder();

                return result;
            }
            else
            {
                Polynomial result = new Polynomial(a_u.m_order);

                for (int i = a_v.m_order + 1; i <= a_u.m_order; i++)
                    result.m_coeffs[i] = a_u.m_coeffs[i];

                for (int i = 0; i <= a_v.m_order; i++)
                    result.m_coeffs[i] = a_u.m_coeffs[i] + a_v.m_coeffs[i];

                result.OptimizeOrder();

                return result;
            }
        }

        public static Polynomial operator -(Polynomial a_poly)
        {
            Polynomial result = new Polynomial(a_poly.m_order);

            for (int i = 0; i <= result.m_order; i++)
                result.m_coeffs[i] = -a_poly.m_coeffs[i];

            return result;
        }

        public static Polynomial operator -(Polynomial a_u, Polynomial a_v)
        {
            if (a_u.m_order == a_v.m_order)
            {
                Polynomial result = new Polynomial(a_u.m_order);

                for (int i = 0; i <= a_u.m_order; i++)
                    result.m_coeffs[i] = a_u.m_coeffs[i] - a_v.m_coeffs[i];

                result.OptimizeOrder();

                return result;
            }
            else if (a_u.m_order < a_v.m_order)
            {
                Polynomial result = new Polynomial(a_v.m_order);

                for (int i = a_u.m_order + 1; i <= a_v.m_order; i++)
                    result.m_coeffs[i] = -a_v.m_coeffs[i];

                for (int i = 0; i <= a_u.m_order; i++)
                    result.m_coeffs[i] = a_u.m_coeffs[i] - a_v.m_coeffs[i];

                result.OptimizeOrder();

                return result;
            }
            else
            {
                Polynomial result = new Polynomial(a_u.m_order);

                for (int i = a_v.m_order + 1; i <= a_u.m_order; i++)
                    result.m_coeffs[i] = a_u.m_coeffs[i];

                for (int i = 0; i <= a_v.m_order; i++)
                    result.m_coeffs[i] = a_u.m_coeffs[i] - a_v.m_coeffs[i];

                result.OptimizeOrder();

                return result;
            }
        }

        public static Polynomial operator %(Polynomial a_u, Polynomial a_v)
        {
            if (a_u.m_order < a_v.m_order)
                return a_u;

            Polynomial reminder = new Polynomial(a_u);

            while (reminder.m_order >= a_v.m_order)
            {
                double d = reminder.LeadCoefficient / a_v.LeadCoefficient;

                for (int j = 0; j <= a_v.m_order; j++)
                {
                    reminder.m_coeffs[reminder.m_order - a_v.m_order + j] -=
                        a_v.m_coeffs[j] * d;
                }

                if (reminder.m_order > 0)
                    reminder.m_order--;
                else
                    break;
            }

            reminder.OptimizeOrder();
            reminder.Check();

            return reminder;
        }

        public static Polynomial operator /(Polynomial a_u, Polynomial a_v)
        {
            Polynomial reminder = new Polynomial(a_u);

            if (a_u.m_order < a_v.m_order)
                return Polynomial.ZERO;

            Polynomial result = new Polynomial(a_u.m_order - a_v.m_order);

            while (reminder.m_order >= a_v.m_order)
            {
                double d = reminder.LeadCoefficient / a_v.LeadCoefficient;
                result.m_coeffs[reminder.m_order - a_v.m_order] = d;

                for (int j = 0; j <= a_v.m_order; j++)
                {
                    reminder.m_coeffs[reminder.m_order - a_v.m_order + j] -=
                        a_v.m_coeffs[j] * d;
                }

                reminder.m_order--;
            }

            result.OptimizeOrder();
            result.Check();

            return result;
        }

        public static Polynomial GCD(Polynomial a_a, Polynomial a_b, 
            double a_precision = Constants.DOUBLE_PRECISION)
        {
            if (a_a.Order < a_b.Order)
            {
                Polynomial t = a_b;
                a_b = a_a;
                a_a = t;
            }

            while (!a_b.IsAlmostEquals(Polynomial.ZERO, a_precision))
            {
                Polynomial t = a_b;

                a_b = a_a % a_b;
                a_a = t;
            }

            if (a_a.LeadCoefficient < 0)
                a_a = -a_a;

            return a_a.Normalize();
        }

        public static Polynomial DivRem(Polynomial a_divider, Polynomial a_divisor, 
            out Polynomial a_reminder)
        {
            a_reminder = new Polynomial(a_divider);

            if (a_divider.m_order < a_divisor.m_order)
                return Polynomial.ZERO;

            Polynomial result = new Polynomial(a_reminder.m_order - a_divisor.m_order);

            while (a_reminder.m_order >= a_divisor.m_order)
            {
                double d = a_reminder.LeadCoefficient / a_divisor.LeadCoefficient;
                result.m_coeffs[a_reminder.m_order - a_divisor.m_order] = d;

                for (int j = 0; j <= a_divisor.m_order; j++)
                {
                    a_reminder.m_coeffs[a_reminder.m_order - a_divisor.m_order + j] -=
                        a_divisor.m_coeffs[j] * d;
                }

                if (a_reminder.m_order > 0)
                    a_reminder.m_order--;
                else
                    break;
            }

            result.OptimizeOrder();
            result.Check();

            a_reminder.OptimizeOrder();
            a_reminder.Check();

            return result;
        }

        public override string ToString()
        {
            string coeffs = "";

            for (int i = 0; i <= m_order; i++)
            {
                coeffs += m_coeffs[i].ToString(CultureInfo.InvariantCulture);

                if (i != m_order)
                    coeffs += ", ";
            }

            return String.Format("Order: {0}, Coeffs: {1}", m_order, coeffs);
        }

        public static Polynomial operator ^(Polynomial a_left, int a_right)
        {
            if (a_right < 0)
            {
                Debug.Fail("");
                return Polynomial.ZERO;
            }

            if (a_right == 0)
                return Polynomial.ONE;

            Polynomial result = new Polynomial(a_left);

            while (a_right > 1)
            {
                a_right--;
                result *= a_left;
            }

            return result;
        }

        public Polynomial RemoveZeroes()
        {
            Polynomial result = new Polynomial(this);

            while (result.m_order > 0 && (result.m_coeffs[0].IsAlmostEquals(0)))
            {
                for (int i = 1; i <= result.m_order; i++)
                    result[i - 1] = result[i];

                result.m_order--;
            }

            return result;
        }

        public IEnumerable<double> Coeffs
        {
            get
            {
                return m_coeffs;
            }
        }
    }
}
